#include "./cargo_motor.h"
#include "../xlib/gpio.h"
#include "../xlib/pwm.h"
#include "../rtos/event_flags.h"
#include "../pub_driver/simple_step_motor_drv.h"
#include "./sensor.h"
#include "../utils/log.h"

namespace cargo_driver {

CargoMotor cargoMotor;

namespace cargo_motor_impl {

using namespace stm32f1;
using namespace rtos::lazy;
using namespace pub_driver;

using PULS = gpio::PA<0>;
using DIR = gpio::PA<5>;
using ENABLE = gpio::PA<7>;
using ALM = gpio::PA<6>;
using PWM = pwm::Timer5PWM<1>;

struct CargoDrvCfg : public SimpleDefaultCfg {

    using PULS_PIN = PULS;

    static void set_dir(SimpleDir dir) {
        switch (dir) {
            case SimpleDir::ENTER:
                DIR::set();
            break;

            case SimpleDir::EXIT:
                DIR::clr();
            break;

            case SimpleDir::DIRECT:
                DIR::set();
            break;
        }
    }

    static void set_pwm_freq(uint32_t freq) {
        PWM::set_freq(freq);
    }

    static void enable_pwm(uint32_t freq) {
        PULS::set_mode<gpio::Mode::OUT_50MHz_AF_PP>();
        PWM::start_with_irq(freq);
    }

    static void enable_pwm_not_irq(uint32_t freq) {
        PULS::set_mode<gpio::Mode::OUT_50MHz_AF_PP>();
        PWM::start_with_no_irq(freq);
    }

    static void disable_pwm() {
        PULS::set_mode<gpio::Mode::OUT_50MHz_PP>();
        PWM::stop();
    }

    static bool is_origin() {
        return sensor.is_cargo_origin();
    }

    static bool is_alm() {
        return !ALM::read();
    }

    static constexpr uint32_t BASE_FREQ = 200;
    static constexpr uint32_t ACC = 2;
    static constexpr uint32_t VAR_RANGE = 200;

    static constexpr uint8_t EC_ALM = ec::cargo::CARGO_ALM;
    static constexpr uint8_t EC_TIMEOUT = ec::cargo::CARGO_TIMEOUT;

    static constexpr uint32_t SLOW_DELAY_MS = 3;

    static constexpr uint32_t RESET_PULS_COUNT = 1000;
    static constexpr uint32_t RESET_TIMEOUT = 30 * 1000;
    static constexpr uint32_t RESET_FREQ = 800;
};

struct MotorInfo {
    bool is_ok = false;
    uint32_t position = 0;
};

static SimpleStepMotorDrv<CargoDrvCfg> drv;
static MotorInfo info;

}

using namespace cargo_motor_impl;

void CargoMotor::init() {

    drv.init();

    rcc::enable<
        PULS, DIR, ENABLE, ALM, PWM
    >();

    gpio::config<
        gpio::Cfg<PULS, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<DIR, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<ENABLE, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<ALM, gpio::Mode::INPUT_FLOAT>
    >();
    ENABLE::clr();
    PWM::stop();
    NVIC_EnableIRQ(TIM5_IRQn);
}

Error CargoMotor::run(uint32_t position) {
    Error err;

    if (is_alm()) {
        info.is_ok = false;
        ENABLE::set();
        osDelay(2000);
        ENABLE::clr();
        osDelay(100);
    }

    if ((!info.is_ok) || (position < info.position)) {
        err = drv.reset();
        if (err) {
            return err;
        }
        info.is_ok = true;
        info.position = 0;
    }

    err = drv.run(SimpleDir::ENTER, position - info.position);

finish:
    if ((err.is_ok() && (position == 0))) {
        err = drv.reset_slow();
    }
    info.is_ok = err.is_ok();
    info.position = position;
    return err;
}

void CargoMotor::test_run_cw(uint16_t freq) {
    DIR::set();
    CargoDrvCfg::enable_pwm_not_irq(freq);
}

void CargoMotor::test_run_ccw(uint16_t freq) {
    DIR::clr();
    CargoDrvCfg::enable_pwm_not_irq(freq);
}

void CargoMotor::test_run_stop() {
    CargoDrvCfg::disable_pwm();
}

bool CargoMotor::is_alm() const {
    return CargoDrvCfg::is_alm();
}

}

using namespace cargo_driver;

extern "C"
void TIM5_IRQHandler() {
    if (PWM::is_uie()) {
        PWM::clear_flag();
        drv.on_puls();
    }
}
